/*
 * @Description: 点云畸变补偿
 * @Author: Zhijian Qiao
 * @Date: 2020-02-25 14:39:00
 */
#include "models/scan_adjust/distortion_adjust.hpp"
#include "glog/logging.h"
#include "CommonFunc.h"
#include "tools/se3_util.hpp"

namespace avp_slam {
    void DistortionAdjust::SetMotionInfo(float scan_period, VelocityData velocity_data) {
        scan_period_ = scan_period;
        velocity_ << velocity_data.linear_velocity.x, velocity_data.linear_velocity.y, velocity_data.linear_velocity.z;
        angular_rate_
                << velocity_data.angular_velocity.x, velocity_data.angular_velocity.y, velocity_data.angular_velocity.z;
    }

    bool DistortionAdjust::AdjustCloud(CloudRingData::CLOUD_PTR &input_cloud_ptr,
                                       CloudRingData::CLOUD_PTR &output_cloud_ptr) { //todo: （暂不处理）这里默认雷达逆时针扫，但实际应该是顺时针
        CloudRingData::CLOUD_PTR origin_cloud_ptr(new CloudRingData::CLOUD(*input_cloud_ptr));
        output_cloud_ptr.reset(new CloudRingData::CLOUD());

        float orientation_space = 2.0 * M_PI;
        float delete_space = 5.0 * M_PI / 180.0;
        float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x);

        Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ());
        Eigen::Matrix3f rotate_matrix = t_V.matrix();
        Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
        transform_matrix.block<3, 3>(0, 0) = rotate_matrix.transpose();
        pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);

        velocity_ = rotate_matrix * velocity_; //todo: 应该×rotate_matrix.transpose? 因为rotate_matrix是同一向量从初始角度坐标变回雷达坐标
        angular_rate_ = rotate_matrix * angular_rate_;

        for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) {
            float orientation = atan2(origin_cloud_ptr->points[point_index].y, origin_cloud_ptr->points[point_index].x);
            if (orientation < 0.0)
                orientation += 2.0 * M_PI;

            if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)
                continue;

            float real_time = fabs(orientation) / orientation_space * scan_period_ -
                              scan_period_ / 2.0;//todo: 为什么减去scan_period_ /2

            Eigen::Vector3f origin_point(origin_cloud_ptr->points[point_index].x,
                                         origin_cloud_ptr->points[point_index].y,
                                         origin_cloud_ptr->points[point_index].z);

            Eigen::Matrix3f current_matrix = UpdateMatrix(real_time);
            Eigen::Vector3f rotated_point = current_matrix * origin_point;
            Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time; // todo: 先旋转再平移似不对，应该乘变换矩阵
            CloudRingData::POINT point;
            point.x = adjusted_point(0);
            point.y = adjusted_point(1);
            point.z = adjusted_point(2);
            output_cloud_ptr->points.push_back(point);
        }

        pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, EigenIsoInv(transform_matrix));
        return true;
    }

    bool
    DistortionAdjust::AdjustCloud(CloudRingData::CLOUD_PTR &input_cloud_ptr, CloudRingData::CLOUD_PTR &output_cloud_ptr,
                                  Eigen::Matrix4f T_adjust) {
        CloudRingData::CLOUD_PTR origin_cloud_ptr(new CloudRingData::CLOUD(*input_cloud_ptr));
        output_cloud_ptr.reset(new CloudRingData::CLOUD());

        float orientation_space = 2.0 * M_PI;
        float delete_space = 5.0 * M_PI / 180.0;
        float start_orientation = atan2(origin_cloud_ptr->points[0].y,
                                        origin_cloud_ptr->points[0].x);//atan2(y,x)返回从+x逆时针旋转的(-pi, pi]
//        LOG(INFO) << "start ori = " << start_orientation;

        Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ());
        Eigen::Matrix3f rotate_matrix = t_V.matrix();
        Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
        transform_matrix.block<3, 3>(0, 0) = rotate_matrix.transpose();
        pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr,
                                 transform_matrix);// transform之后-atan2(y,x)返回从激光雷达+y轴顺时针为正的[-pi, pi)

        T_adjust = transform_matrix * T_adjust * EigenIsoInv(transform_matrix);

        for (size_t point_index = 0; point_index < origin_cloud_ptr->points.size(); ++point_index) {
            float orientation = -atan2(origin_cloud_ptr->points[point_index].y,
                                       origin_cloud_ptr->points[point_index].x);//认为雷达旋转方向为俯视顺时针
            if (orientation < 0.0)
                orientation += 2.0 * M_PI;

//            if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)
//                continue;

            float alpha =
                    orientation / orientation_space; // no need to -start_orientation since points already transformed

//            LOG(INFO) << "point " << point_index << " alpha = " << alpha;

            Eigen::Vector4f origin_point(origin_cloud_ptr->points[point_index].x,
                                         origin_cloud_ptr->points[point_index].y,
                                         origin_cloud_ptr->points[point_index].z,
                                         1.0);

            Eigen::Matrix4f M0 = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f current_matrix = interpolateSE3(M0, T_adjust, alpha);
            Eigen::Vector4f adjusted_point = current_matrix * origin_point;
            CloudRingData::POINT point;
            point.x = adjusted_point(0);
            point.y = adjusted_point(1);
            point.z = adjusted_point(2);
            output_cloud_ptr->points.push_back(point);
        }

        pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, EigenIsoInv(transform_matrix));
        return true;
    }

    Eigen::Matrix3f DistortionAdjust::UpdateMatrix(float real_time) {
        Eigen::Vector3f angle = angular_rate_ * real_time;
        Eigen::AngleAxisf t_Vz(angle(2), Eigen::Vector3f::UnitZ());
        Eigen::AngleAxisf t_Vy(angle(1), Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf t_Vx(angle(0), Eigen::Vector3f::UnitX());
        Eigen::AngleAxisf t_V;
        t_V = t_Vz * t_Vy * t_Vx;
        return t_V.matrix();
    }
} // namespace avp_slam